﻿using PropertyChanged;
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Drawing.Imaging;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace VIA.SiMoJi.Domain.Vision
{
    [AddINotifyPropertyChangedInterface]
    public class DuiWeiAlgorithmParam
    {
        /// <summary>
        /// 标定矩阵
        /// </summary>
        public CMatrix Matrix { get; set; } = new();
        /// <summary>
        /// 旋转中心
        /// </summary>
        public RotationCenter RotationCenter { get; set; } = new();
        /// <summary>
        /// 机器人拍Mark点位
        /// </summary>
        public Pose MarkPose { get; set; } = new();
        /// <summary>
        /// 保存九点标定时点间隔和旋转标定时的角度
        /// </summary>
        public Pose CalibrationOffset { get; set; } = new Pose(10, 10, 5);
        /// <summary>
        /// 补偿值
        /// </summary>
        public Pose Compensation { get; set; } = new();
        /// <summary>
        /// 九点标定时的机器人和图像坐标对
        /// </summary>
        public ObservableCollection<RobotImagePoseGroup> CalibrateRobotImagePoseGroups { get; set; } = new ObservableCollection<RobotImagePoseGroup>();
        /// <summary>
        /// 旋转中心标定时的机器人和图像坐标对
        /// </summary>
        public ObservableCollection<RobotImagePoseGroup> RotationRobotImagePoseGroups { get; set; } = new ObservableCollection<RobotImagePoseGroup>();
        /// <summary>
        /// 反示教的机器人和图像坐标对
        /// </summary>
        public ObservableCollection<RobotImagePoseGroup> TeachRobotImagePoseGroups { get; set; } = new ObservableCollection<RobotImagePoseGroup>();

        public Pose GetRocationCalibrationPose()
        {
            return RotationRobotImagePoseGroups[0].RobotPose;
        }

        public static (double cx, double cy, double a) GetCoordinatePoint(CMatrix matrix, RobotImagePoseGroup destRobotImagePoint, RobotImagePoseGroup originRobotImagePoint)
        {
            var originWordPoint = matrix.Transform(originRobotImagePoint.ImagePose);
            var destWordPoint = matrix.Transform(destRobotImagePoint.ImagePose);
            double cx = destWordPoint.X - originWordPoint.X + destRobotImagePoint.RobotPose.X - originRobotImagePoint.RobotPose.X;
            double cy = destWordPoint.Y - originWordPoint.Y + destRobotImagePoint.RobotPose.Y - originRobotImagePoint.RobotPose.Y;
            double a = destRobotImagePoint.ImagePose.Angle - originRobotImagePoint.ImagePose.Angle;
            return (cx, cy, a);
        }

        public static bool CaculateCenterAndR(double x1, double y1, double x2, double y2, double x3, double y3, out double cx, out double cy, out double r)
        {
            cx = 0; cy = 0; r = 0;
            double sx1 = x1 * x1;
            double sy1 = y1 * y1;
            double sx2 = x2 * x2;
            double sy2 = y2 * y2;
            double sx3 = x3 * x3;
            double sy3 = y3 * y3;
            double A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2;
            if (A == 0) return false;
            double B = (sx1 + sy1) * (y3 - y2) + (sx2 + sy2) * (y1 - y3) + (sx3 + sy3) * (y2 - y1);
            double C = (sx1 + sy1) * (x2 - x3) + (sx2 + sy2) * (x3 - x1) + (sx3 + sy3) * (x1 - x2);
            double D = (sx1 + sy1) * (x3 * y2 - x2 * y3) + (sx2 + sy2) * (x1 * y3 - x3 * y1) + (sx3 + sy3) * (x2 * y1 - x1 * y2);
            cx = -B / (A * 2);
            cy = -C / (A * 2);
            r = Math.Sqrt((B * B + C * C - 4 * A * D) / (4 * A * A));
            return true;
        }
    }
}
